#!/usr/bin/env python

import Image
import pyglet
import serial
import StringIO
import struct
import sys
import time

class Robot:
    """Robot class. For each instruction you send to the robot, you
    need to empty its output buffer using parse_output().  
    Note that using serial.readlines() has a timeout, making the
    robot seemingly slow to respond. Use serial.readline() instead."""
    def __init__(self):
        self.image = ""

    def connect(self, port, bytesize = 8, baudrate = 115200, timeout = 1):
        """Connect to the robot through a virtual serial-port (Bluetooth)."""
        try:
            self.com = serial.Serial(port, baudrate,
                bytesize, timeout = timeout)
        except serial.serialutil.SerialException:
            sys.stderr.write("*** No connection. Robot appears to be offline\n")
            sys.exit(1)
        print "*** Connected to robot"
        self.init()

    def disconnect(self):
        """Disconnect from robot."""
        print "*** Disconnected"
        self.com.close()

    def init(self):
        """The robot never executes the first instruction it receives."""
        self.com.write("\n")
        output = self.parse_output(self.com.readline())

    def parse_output(self, data):
        """Parse the output from the robot."""
        if data:
            if data[0] is "z":
                pass
            else:
                print data

    def capture_image(self):
        """Capture raw 5:6:5 RGB image data from the camera
        and convert to PNG for compatbility."""
        try:
            instruction = struct.pack(">bb", - ord("I"), 0)
            self.com.write(instruction)
            output = self.com.read(999999)
        except OSError: 
            sys.stderr.write("Lost connection!\n")
            sys.exit(1)
        try:
            buffer = Image.frombuffer("RGB", (40, 40), output,
                                      "raw", "BGR;16", 0, 1)
        except ValueError, msg:
            print "Bad connection: %s" % msg
            return
        buffer = buffer.rotate(180)
        buffer = buffer.resize((160, 160))
        file = StringIO.StringIO()
        buffer.convert("RGBA")
        buffer.save(file, "png")
        self.image = pyglet.image.load("capture.png", file = file)

    def tick(self, interval):
        """This is called periodically throughout the robot's lifetime."""
        self.capture_image()

if __name__ == "__main__":
    robot = Robot()
    robot.connect("/dev/tty.e-puck_1251-COM1-1")

    window = pyglet.window.Window(160, 160)

    robot.tick(0)

    pyglet.clock.schedule_interval(robot.tick, 0.25)

    @window.event
    def on_draw():
        window.clear()
        robot.image.blit(0, 0)

    pyglet.app.run()

    robot.disconnect()
